/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>

#include "../algorithm/filter.h"
#include "../bsp/bsp_usart.h"
#include "../device/icm20602.h"
#include "FreeRTOS.h"
#include "usertask.h"

/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
#define IMU_FREQ 1000u

/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
RawICM_t raw;
Euler_t angel_raw;
Euler_t angel;
Euler_t angel_2;
// Cali_t cali = {
//  .icm20602 = {
//		.gyro_offset ={
//		 .x=-0.767f,
//		 .y=0.583f,
//		 .z=0.237f,
//		},
//		.acc_offset ={
//		 .x=-0.095f,
//		 .y=(0.9645-1),
//		 .z=(0.0432),
//		},
//},
//	};
Cali_t cali;
static AHRS_t ahrs = {
    .quat =
        {
            .q0 = 1.0f,
            .q1 = 0.0f,
            .q2 = 0.0f,
            .q3 = 0.0f,
        },
    .samplefreq = 1000u,
};

/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */

void ImuTask(void *argument) {
  (void)argument;
  const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ;
  ICM_init();

  uint32_t tick = osKernelGetTickCount();
  while (1) {
    tick += delay_tick;
    if (cali.icm_init) {
      ICM_GetAcc(&raw);
      ICM_GetGyro(&raw);
      RawFloat_t raw_f = ICM_Int16toFloat(&raw, &cali);

      IMU_Prase(&ahrs, &angel, &angel_2, &angel_raw, &raw_f);
    }
    osDelayUntil(tick);
  }
}
